#include "CrowdSimFrameListener.h"

CrowdSimFrameListener::CrowdSimFrameListener(
	SceneManager *sceneMgr, 
	RenderWindow* win, 
	Camera* cam,
	Vector3 &gravityVector,
	AxisAlignedBox &bounds)
	: ExampleFrameListener(win, cam),
	mSceneMgr(sceneMgr)
{
	mMoveSpeed = 50;	// defined in ExampleFrameListener
	mNumEntitiesInstanced = 0; // how many shapes are created
	mSceneMgr = sceneMgr;
	// Start Bullet
	mWorld = new OgreBulletDynamics::DynamicsWorld(mSceneMgr, bounds, gravityVector);

	// add Debug info display tool
	debugDrawer = new OgreBulletCollisions::DebugDrawer();
	debugDrawer->setDrawWireframe(true);	// we want to see the Bullet containers

	mWorld->setDebugDrawer(debugDrawer);
	mWorld->setShowDebugShapes(true);		// enable it if you want to see the Bullet containers
	SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode("debugDrawer", Ogre::Vector3::ZERO);
	node->attachObject(static_cast <SimpleRenderable *> (debugDrawer));

	// Define a floor plane mesh
	Entity *ent;
	Plane p;
	p.normal = Vector3(0,1,0); p.d = 0;
	MeshManager::getSingleton().createPlane("FloorPlane", 
		ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, 
		p, 200000, 200000, 20, 20, true, 1, 9000, 9000, 
		Vector3::UNIT_Z);
	// Create an entity (the floor)
	ent = mSceneMgr->createEntity("floor", "FloorPlane");
	ent->setMaterialName("Examples/BumpyMetal");
	mSceneMgr->getRootSceneNode()->createChildSceneNode()->attachObject(ent);

	// add collision detection to it
	OgreBulletCollisions::CollisionShape *Shape;
	Shape = new OgreBulletCollisions::StaticPlaneCollisionShape(Ogre::Vector3(0,1,0), 0); // (normal vector, distance)
	// a body is needed for the shape
	OgreBulletDynamics::RigidBody *defaultPlaneBody = new OgreBulletDynamics::RigidBody("BasePlane",
		mWorld);
	defaultPlaneBody->setStaticShape(Shape, 0.1, 0.8);// (shape, restitution, friction)
	// push the created objects to the deques
	mShapes.push_back(Shape);
	mBodies.push_back(defaultPlaneBody);

	readRoadMap();
	buildRoadMap();

}
CrowdSimFrameListener::~CrowdSimFrameListener(){
	// OgreBullet physic delete - RigidBodies
	std::deque<OgreBulletDynamics::RigidBody *>::iterator itBody = mBodies.begin();
	while (mBodies.end() != itBody)
	{   
		delete *itBody; 
		++itBody;
	}	
	// OgreBullet physic delete - Shapes
	std::deque<OgreBulletCollisions::CollisionShape *>::iterator itShape = mShapes.begin();
	while (mShapes.end() != itShape)
	{   
		delete *itShape; 
		++itShape;
	}
	mBodies.clear();
	mShapes.clear();
	delete mWorld->getDebugDrawer();
	mWorld->setDebugDrawer(0);
	delete mWorld;
}

void CrowdSimFrameListener::readRoadMap()
{
	RoadmapVertex v;
	v.position = Vector3(-30,10,90);
	roadmap.push_back(v);
	v.position = Vector3(-50,10,-50);
	roadmap.push_back(v);
	v.position = Vector3(110,10,-30);
	roadmap.push_back(v);
	v.position = Vector3(200,10,-200);
	roadmap.push_back(v);
}

void CrowdSimFrameListener::buildRoadMap()
{
	/* Connect the roadmap vertices by edges if mutually visible. */
#pragma omp parallel for
	for (int i = 0; i < static_cast<int>(roadmap.size()); ++i) {
		for (int j = 0; j < static_cast<int>(roadmap.size()); ++j) {
			if (queryVisibility(roadmap[i].position, roadmap[j].position)) {
				roadmap[i].neighbors.push_back(j);
			}
		}

		/* 
		* Initialize the distance to each of the four goal vertices at infinity
		* (9e9f).
		*/
		roadmap[i].distToGoal.resize(4, 9e9f);
	}

	/*
	* Compute the distance to each of the four goals (the first four vertices)
	* for all vertices using Dijkstra's algorithm.
	*/
#pragma omp parallel for
	for (int i = 0; i < 4; ++i) {
		std::multimap<float, int> Q;
		std::vector<std::multimap<float, int>::iterator> posInQ(roadmap.size(), Q.end());

		roadmap[i].distToGoal[i] = 0.0f;
		posInQ[i] = Q.insert(std::make_pair(0.0f, i));

		while (!Q.empty()) {
			const int u = Q.begin()->second;
			Q.erase(Q.begin());
			posInQ[u] = Q.end();

			for (int j = 0; j < static_cast<int>(roadmap[u].neighbors.size()); ++j) {
				const int v = roadmap[u].neighbors[j];
				const float dist_uv = abs(roadmap[v].position - roadmap[u].position);

				if (roadmap[v].distToGoal[i] > roadmap[u].distToGoal[i] + dist_uv) {
					roadmap[v].distToGoal[i] = roadmap[u].distToGoal[i] + dist_uv;

					if (posInQ[v] == Q.end()) {
						posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v));
					} else {
						Q.erase(posInQ[v]);
						posInQ[v] = Q.insert(std::make_pair(roadmap[v].distToGoal[i], v));
					}
				}
			}
		}
	}
}

bool CrowdSimFrameListener::queryVisibility(Vector3 a, Vector3 b)
{
	//mWorld->
}

bool CrowdSimFrameListener::frameStarted(const FrameEvent& evt)
{
	bool ret = ExampleFrameListener::frameStarted(evt);

	mWorld->stepSimulation(evt.timeSinceLastFrame);	// update Bullet Physics animation

	return ret;
}

bool CrowdSimFrameListener::frameEnded(const FrameEvent& evt)
{
	bool ret = ExampleFrameListener::frameEnded(evt);

	mWorld->stepSimulation(evt.timeSinceLastFrame);	// update Bullet Physics animation

	return ret;
}

bool CrowdSimFrameListener::processUnbufferedKeyInput(const FrameEvent& evt)
{
	bool ret = ExampleFrameListener::processUnbufferedKeyInput(evt);

	// create and throw a box if 'B' is pressed
	if(mKeyboard->isKeyDown(OIS::KC_B) && mTimeUntilNextToggle <=0)
	{
		Vector3 size = Vector3::ZERO;	// size of the box
		// starting position of the box
		Vector3 position = (mCamera->getDerivedPosition() + mCamera->getDerivedDirection().normalisedCopy() * 10);

		// create an ordinary, Ogre mesh with texture
		Entity *entity = mSceneMgr->createEntity(
			"Box" + StringConverter::toString(mNumEntitiesInstanced),
			"cube.mesh");			    
		entity->setCastShadows(true);
		// we need the bounding box of the box to be able to set the size of the Bullet-box
		AxisAlignedBox boundingB = entity->getBoundingBox();
		size = boundingB.getSize(); size /= 2.0f; // only the half needed
		//size *= 0.95f;	// Bullet margin is a bit bigger so we need a smaller size
		// (Bullet 2.76 Physics SDK Manual page 18)
		entity->setMaterialName("Examples/BumpyMetal");
		SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode();
		node->attachObject(entity);
		node->scale(0.05f, 0.05f, 0.05f);	// the cube is too big for us
		size *= 0.05f;						// don't forget to scale down the Bullet-box too

		// after that create the Bullet shape with the calculated size
		OgreBulletCollisions::BoxCollisionShape *sceneBoxShape = new OgreBulletCollisions::BoxCollisionShape(size);
		// and the Bullet rigid body
		defaultBody = new OgreBulletDynamics::RigidBody(
			"defaultBoxRigid" + StringConverter::toString(mNumEntitiesInstanced), 
			mWorld);
		defaultBody->setShape(	node,
			sceneBoxShape,
			0.6f,			// dynamic body restitution
			0.6f,			// dynamic body friction
			1.0f, 			// dynamic body mass
			position,		// starting position of the box
			Quaternion(0,0,0,1));// orientation of the box
		mNumEntitiesInstanced++;				

		defaultBody->setLinearVelocity(
			mCamera->getDerivedDirection().normalisedCopy() * 7.0f ); // shooting speed
		// push the created objects to the dequese
		mShapes.push_back(sceneBoxShape);
		mBodies.push_back(defaultBody);				
		mTimeUntilNextToggle = 0.5;
	}
	if(mKeyboard->isKeyDown(OIS::KC_V))
	{
		defaultBody->setLinearVelocity(Vector3(0.0f, -1.0f, 0.0f));
	}
	return ret;
}